home *** CD-ROM | disk | FTP | other *** search
/ Tech Arsenal 1 / Tech Arsenal (Arsenal Computer).ISO / tek-01 / dkbsrc.zip / VECT.C < prev    next >
C/C++ Source or Header  |  1991-05-10  |  17KB  |  633 lines
  1. /*****************************************************************************
  2. *
  3. *                                   vect.c
  4. *
  5. *   from DKBTrace (c) 1990  David Buck
  6. *
  7. *  This module implements additional quartic math-helping functions.
  8. *
  9. *  This file was written by Alexander Enzmann.  He wrote the code for DKB 2.11
  10. *  QUARTICS (4th order shapes) and generously provided us these enhancements.
  11. *
  12. * This software is freely distributable. The source and/or object code may be
  13. * copied or uploaded to communications services so long as this notice remains
  14. * at the top of each file.  If any changes are made to the program, you must
  15. * clearly indicate in the documentation and in the programs startup message
  16. * who it was who made the changes. The documentation should also describe what
  17. * those changes were. This software may not be included in whole or in
  18. * part into any commercial package without the express written consent of the
  19. * author.  It may, however, be included in other public domain or freely
  20. * distributed software so long as the proper credit for the software is given.
  21. *
  22. * This software is provided as is without any guarantees or warranty. Although
  23. * the author has attempted to find and correct any bugs in the software, he
  24. * is not responsible for any damage caused by the use of the software.  The
  25. * author is under no obligation to provide service, corrections, or upgrades
  26. * to this package.
  27. *
  28. * Despite all the legal stuff above, if you do find bugs, I would like to hear
  29. * about them.  Also, if you have any comments or questions, you may contact me
  30. * at the following address:
  31. *
  32. *     David Buck
  33. *     22C Sonnet Cres.
  34. *     Nepean Ontario
  35. *     Canada, K2H 8W7
  36. *
  37. *  I can also be reached on the following bulleton boards:
  38. *
  39. *     OMX              (613) 731-3419
  40. *     Mystic           (613) 596-4249  or  (613) 596-4772
  41. *
  42. *  Fidonet:   1:163/109.9
  43. *  Internet:  dbuck@ccs.carleton.ca
  44. *  The "You Can Call Me RAY" BBS    (708) 358-5611
  45. *
  46. *  IBM Port by Aaron A. Collins. Aaron may be reached on the following BBS'es:
  47. *
  48. *     The "You Can Call Me RAY" BBS (708) 358-5611
  49. *     The Information Exchange BBS  (708) 945-5575
  50. *
  51. *****************************************************************************/
  52.  
  53. #include "frame.h"
  54. #include "dkbproto.h"
  55.  
  56. #ifdef ABS
  57. #undef ABS
  58. #endif
  59.  
  60. #define ABS(x) ((x) < 0.0 ? (0.0 - x) : (x))
  61.  
  62. #ifndef PI
  63. #define PI   3.141592653589793238462643383280
  64. #endif
  65.  
  66. #ifndef TWO_PI
  67. #define TWO_PI 6.283185207179586476925286766560
  68. #endif
  69.  
  70. #define TWO_PI_3  2.0943951023931954923084
  71. #define TWO_PI_43 4.1887902047863909846168
  72. #define MINUS_INFINITY -1.0e10
  73. #define POSITIVE_INFINITY 1.0e10
  74.  
  75. #ifdef TEST
  76. void
  77. show_poly(label, order, Coeffs)
  78.    char *label;
  79.    int order;
  80.    DBL *Coeffs;
  81. {
  82.    int i;
  83.    printf("%s", label);
  84.    for (i=0;i<=order;i++) {
  85.       printf("%.2lf x^%d", Coeffs[i], order-i);
  86.       if (i==order) printf("\n");
  87.       else printf(" + ");
  88.       }
  89. }
  90.  
  91. void
  92. show_points(label, count, point_list)
  93.    char *label;
  94.    int count;
  95.    DBL *point_list;
  96. {
  97.    int i;
  98.    printf("%s", label);
  99.    for (i=0;i<count;i++) {
  100.       printf("%lf", point_list[i]);
  101.       if (i==(count-1)) printf("\n");
  102.       else printf(", ");
  103.       }
  104. }
  105.  
  106. /* Deflate a polynomial by the factor (x - a). */
  107. void
  108. deflate(n, coeffs, a)
  109.    int n;
  110.    DBL *coeffs;
  111.    DBL a;
  112. {
  113.    register int i;
  114.    DBL temp, rem = coeffs[0];
  115.    coeffs[0] = 0.0;
  116.    for (i=1;i<=n;i++) {
  117.       temp = coeffs[i];
  118.       coeffs[i] = rem;
  119.       rem = temp + rem * a;
  120.       }
  121. }
  122.  
  123. /* find the coefficients of the derivative of a polynomial */
  124. void
  125. dydx(order, coeffs, dcoeffs)
  126.    int order;
  127.    DBL *coeffs;
  128.    DBL *dcoeffs;
  129. {
  130.    register int i;
  131.    for (i=0;i<order;i++)
  132.       dcoeffs[i] = (order - i) * coeffs[i];
  133. }
  134.  
  135. #define MAX_ITERATIONS 100
  136. typedef void (*polyfunction) PARAMS((DBL x, int n, DBL *Coeffs, DBL *val, DBL *dx));
  137.  
  138. void
  139. polyeval(x, n, Coeffs, val, dx)
  140.    DBL x
  141.    int n;
  142.    DBL *Coeffs, *val, *dx;
  143. {
  144.    register int i;
  145.    *val = Coeffs[0];
  146.    *dx = 0.0;
  147.    for (i=1;i<=n;i++) {
  148.       *dx = *dx * x + *val;
  149.       *val = *val * x + Coeffs[i];
  150.       }
  151. }
  152.  
  153. /* Solve for a single root of a polynomial, using a bracketed
  154.    Newton-Raphson technique. 1 = root found, 0 = failure. */
  155. int
  156. rtsafe(funcd, n, Coeffs, x1, x2, epsilon, result)
  157.    polyfunction funcd;
  158.    int n;
  159.    DBL *Coeffs, x1, x2, epsilon, *result;
  160. {
  161.    int j;
  162.    DBL df, dx, dxold, f, fh, fl;
  163.    DBL swap, temp, xh, xl, rts;
  164.    (*funcd)(x1, n, Coeffs, &fl, &df);
  165.    (*funcd)(x2, n, Coeffs, &fh, &df);
  166.    if (ABS(fl) < epsilon) {
  167.       /* Root is on the lower bound */
  168.       *result = x1;
  169.       return 1;
  170.       }
  171.    else if (ABS(fh) < epsilon) {
  172.       /* Root is on the upper bound */
  173.       *result = x2;
  174.       return 1;
  175.       }
  176.    else if (fl * fh >= 0.0) {
  177.       /* Bad bounds - function should change signs over the given interval */
  178.       *result = 0.0;
  179.       return 0;
  180.       }
  181.    if (fl < 0.0) {
  182.       xl = x1;
  183.       xh = x2;
  184.       }
  185.    else {
  186.       xh = x1;
  187.       xl = x2;
  188.       swap = fl;
  189.       fl = fh;
  190.       fh = swap;
  191.       }
  192.    rts = 0.5 * (x1 + x2);
  193.    dxold = ABS(x2-x1);
  194.    dx = dxold;
  195.    (*funcd)(rts, n, Coeffs, &f, &df);
  196.    for (j=1;j<=MAX_ITERATIONS;j++) {
  197.       if ((((rts-xh)*df-f)*((rts-xl)*df-f) >= 0.0) ||
  198.      /* Newton out of range, bisect instead */
  199.       (ABS(2.0*f) > ABS(dxold*df))) {
  200.      /* Newton not converging fast enough, bisect instead */
  201.      dxold = dx;
  202.      dx = 0.5 * (xh-xl);
  203.      rts = xl + dx;
  204.      if (xl == rts) {
  205.         *result = rts;
  206.         return 1;
  207.         }
  208.      }
  209.       else {
  210.      dxold = dx;
  211.      dx = f / df;
  212.      temp = rts;
  213.      rts -= dx;
  214.      if (temp == rts) {
  215.         *result = rts;
  216.         return 1;
  217.         }
  218.      }
  219.       if (ABS(dx) < epsilon) {
  220.      *result = rts;
  221.      return 1;
  222.      }
  223.       (*funcd)(rts, n, Coeffs, &f, &df);
  224.       if (f < 0.0) {
  225.      xl = rts;
  226.      fl = f;
  227.      }
  228.       else {
  229.      xh = rts;
  230.      fh = f;
  231.      }
  232.       }
  233.    *result = 0.0;
  234.    return 0;
  235. }
  236. #endif
  237.  
  238. /*
  239.    Solve the quadratic equation:
  240.       x[0] * x^2 + x[1] * x + x[2] = 0.
  241.  
  242.    The value returned by this function is the number of real roots.
  243.    The roots themselves are returned in y[0], y[1].
  244. */
  245. solve_quadratic(x, y)
  246.    DBL *x, *y;
  247. {
  248.    DBL d, t, a, b, c;
  249.    a = x[0];
  250.    b = -x[1];
  251.    c = x[2];
  252.    if (a == 0.0) {
  253.       if (b == 0.0)
  254.          return 0;
  255.       y[0] = -c / b;
  256.       return 1;
  257.       }
  258.    d = b * b - 4.0 * a * c;
  259.    if (d < 0.0) return 0;
  260.    d = sqrt(d);
  261.    t = 2.0 * a;
  262.    y[0] = (b + d) / t;
  263.    y[1] = (b - d) / t;
  264.    return 2;
  265. }
  266.  
  267. /*
  268.    Solve the cubic equation:
  269.  
  270.       x[0] * x^3 + x[1] * x^2 + x[2] * x + x[3] = 0.
  271.  
  272.    The result of this function is an integer that tells how many real
  273.    roots exist.  Determination of how many are distinct is up to the
  274.    process that calls this routine.  The roots that exist are stored
  275.    in (y[0], y[1], y[2]).
  276.  
  277.    Note: this function relies very heavily on trigonometric functions and
  278.    the square root function.  If an alternative solution is found that does
  279.    not rely on transcendentals this code will be replaced.
  280. */
  281. int
  282. solve_cubic(x, y)
  283.    DBL *x, *y;
  284. {
  285.    DBL Q, R, Q3, R2, sQ, d, an, theta;
  286.    DBL A2, a0, a1, a2, a3;
  287.    a0 = x[0];
  288.    if (a0 == 0.0) return solve_quadratic(&x[1], y);
  289.    else if (a0 != 1.0) {
  290.       a1 = x[1] / a0;
  291.       a2 = x[2] / a0;
  292.       a3 = x[3] / a0;
  293.       }
  294.    else {
  295.       a1 = x[1];
  296.       a2 = x[2];
  297.       a3 = x[3];
  298.       }
  299.    A2 = a1 * a1;
  300.    Q = (A2 - 3.0 * a2) / 9.0;
  301.    R = (2.0 * A2 * a1 - 9.0 * a1 * a2 + 27.0 * a3) / 54.0;
  302.    Q3 = Q * Q * Q;
  303.    R2 = R * R;
  304.    d = Q3 - R2;
  305.    an = a1 / 3.0;
  306.    if (d >= 0.0) {
  307.       /* Three real roots. */
  308.       d = R / sqrt(Q3);
  309.       theta = acos(d) / 3.0;
  310.       sQ = -2.0 * sqrt(Q);
  311.       y[0] = sQ * cos(theta) - an;
  312.       y[1] = sQ * cos(theta + TWO_PI_3) - an;
  313.       y[2] = sQ * cos(theta + TWO_PI_43) - an;
  314.       return 3;
  315.       }
  316.    else {
  317.       sQ = pow(sqrt(R2 - Q3) + ABS(R), 1.0 / 3.0);
  318.       if (R < 0)
  319.      y[0] = (sQ + Q / sQ) - an;
  320.       else
  321.      y[0] = -(sQ + Q / sQ) - an;
  322.       return 1;
  323.       }
  324. }
  325.  
  326. /*
  327.    Solve the quartic equation:
  328.       x[0] x^4 + x[1] x^3 + x[2] x^2 + x[3] x + x[4] = 0
  329.    The value of this function is the number of real roots. The roots
  330.    themselves are returned in results[0-3].
  331.  
  332.    Two methods of implementation are given, the first is faster by
  333.    than the second by approximately 7-8% (with both in unoptimized form).
  334.    Hand optimization of the first function has been implemented, resulting
  335.    in a boost of around 15% of its speed.
  336. */
  337.  
  338.  
  339. #ifdef TEST
  340. /* Solve a quartic using the method of Lodovico Ferrari (Circa 1731) */
  341. int
  342. solve_quartic(x, results)
  343.    DBL *x, *results;
  344. {
  345.    DBL cubic[4], roots[3];
  346.    DBL a0, a1, y, d1, x1, t1, t2;
  347.    DBL c0, c1, c2, c3, c4, d2, q1, q2;
  348.    int i;
  349.  
  350.    c0 = x[0];
  351.    if (c0 == 0.0)
  352.       return solve_cubic(&x[1], results);
  353.    else if (x[4] == 0.0) {
  354.       results[0] = 0.0;
  355.       return 1 + solve_cubic(x, &results[1]);
  356.       }
  357.    else if (c0 != 1.0) {
  358.       c1 = x[1] / c0;
  359.       c2 = x[2] / c0;
  360.       c3 = x[3] / c0;
  361.       c4 = x[4] / c0;
  362.       }
  363.    else {
  364.       c1 = x[1];
  365.       c2 = x[2];
  366.       c3 = x[3];
  367.       c4 = x[4];
  368.       }
  369.  
  370.    /* The first step is to take the original equation:
  371.  
  372.      x^4 + b*x^3 + c*x^2 + d*x + e = 0
  373.  
  374.       and rewrite it as:
  375.  
  376.      x^4 + b*x^3 = -c*x^2 - d*x - e,
  377.  
  378.       adding (b*x/2)^2 + (x^2 + b*x/2)y + y^2/4 to each side gives a
  379.       perfect square on the lhs:
  380.  
  381.      (x^2 + b*x/2 + y/2)^2 = (b^2/4 - c + y)x^2 + (b*y/2 - d)x + y^2/4 - e
  382.  
  383.       By choosing the appropriate value for y, the rhs can be made a perfect
  384.       square also.  This value is found when the rhs is treated as a quadratic
  385.       in x with the discriminant equal to 0.  This will be true when:
  386.  
  387.      (b*y/2 - d)^2 - 4.0 * (b^2/4 - c*y)*(y^2/4 - e) = 0, or
  388.  
  389.      y^3 - c*y^2 + (b*d - 4*e)*y - b^2*e + 4*c*e - d^2 = 0.
  390.  
  391.       This is called the resolvent of the quartic equation.  */
  392.  
  393.    a0 = 4.0 * c4;
  394.    cubic[0] = 1.0;
  395.    cubic[1] = -1.0 * c2;
  396.    cubic[2] = c1 * c3 - a0;
  397.    cubic[3] = a0 * c2 - c1 * c1 * c4 - c3 * c3;
  398.    i = solve_cubic(&cubic[0], &roots[0]);
  399.    if (i == 3)
  400.       y = max(roots[0], max(roots[1], roots[2]));
  401.    else
  402.       y = roots[0];
  403.  
  404.    /* What we are left with is a quadratic squared on the lhs and a
  405.       linear term on the right.  The linear term has one of two signs,
  406.       take each and add it to the lhs.  The form of the quartic is now:
  407.  
  408.      a' = b^2/4 - c + y,    b' = b*y/2 - d, (from rhs quadritic above)
  409.  
  410.      (x^2 + b*x/2 + y/2) = +sqrt(a'*(x + 1/2 * b'/a')^2), and
  411.      (x^2 + b*x/2 + y/2) = -sqrt(a'*(x + 1/2 * b'/a')^2).
  412.  
  413.       By taking the linear term from each of the right hand sides and
  414.       adding to the appropriate part of the left hand side, two quadratic
  415.       formulas are created.  By solving each of these the four roots of
  416.       the quartic are determined.
  417.    */
  418.    i = 0;
  419.    a0 = c1 / 2.0;
  420.    a1 = y / 2.0;
  421.  
  422.    t1 = a0 * a0 - c2 + y;
  423.    if (t1 < 0.0) {
  424.       /* First Special case, a' < 0 means all roots are complex. */
  425.     if (t1 > -EPSILON)
  426.         t1 = 0;
  427.     else return 0;
  428.     }
  429.    if (t1 == 0) {
  430.       /* Second special case, the "x" term on the right hand side above
  431.      has vanished.  In this case:
  432.         (x^2 + b*x/2 + y/2) = +sqrt(y^2/4 - e), and
  433.         (x^2 + b*x/2 + y/2) = -sqrt(y^2/4 - e).  */
  434.       t2 = a1 * a1 - c4;
  435.       if (t2 < 0.0) return 0;
  436.       x1 = 0.0;
  437.       d1 = sqrt(t2);
  438.       }
  439.    else {
  440.       x1 = sqrt(t1);
  441.       d1 = 0.5 * (a0 * y - c3) / x1;
  442.       }
  443.    /* Solve the first quadratic */
  444.    q1 = -a0 - x1;
  445.    q2 = a1 + d1;
  446.    d2 = q1 * q1 - 4.0 * q2;
  447.    if (d2 >= 0.0) {
  448.       d2 = sqrt(d2);
  449.       results[0] = 0.5 * (q1 + d2);
  450.       results[1] = 0.5 * (q1 - d2);
  451.       i = 2;
  452.       }
  453.    /* Solve the second quadratic */
  454.    q1 = q1 + x1 + x1;
  455.    q2 = a1 - d1;
  456.    d2 = q1 * q1 - 4.0 * q2;
  457.    if (d2 >= 0.0) {
  458.       d2 = sqrt(d2);
  459.       results[i++] = 0.5 * (q1 + d2);
  460.       results[i++] = 0.5 * (q1 - d2);
  461.       }
  462.    return i;
  463. }
  464.  
  465. #endif
  466.  
  467.  
  468. /* Solve a quartic using the method of Francois Vieta (Circa 1735) */
  469. int
  470. solve_quartic(x, results)
  471.    DBL *x, *results;
  472. {
  473.    DBL cubic[4], roots[3];
  474.    DBL A2, y, z, p, q, r, d1, d2;
  475.    int i, j, k;
  476.  
  477.    if (x[0] == 0.0)
  478.       return solve_cubic(&x[1], results);
  479.    else if (x[4] == 0.0) {
  480.       results[0] = 0.0;
  481.       return 1 + solve_cubic(x, &results[1]);
  482.       }
  483.    else if (x[0] != 1.0)
  484.       for (i=1;i<5;i++) x[i] /= x[0];
  485.  
  486.    /* Compute the cubic resolvant */
  487.    A2 = x[1] * x[1];
  488.    p = -3.0 * A2 / 8.0 + x[2];
  489.    q = A2 * x[1] / 8.0 - x[1] * x[2] / 2.0 + x[3];
  490.    r = -3.0 * A2 * A2 / 256.0 + A2 * x[2] / 16.0 - x[1] * x[3] / 4.0 + x[4];
  491.  
  492.    cubic[0] = 1.0;
  493.    cubic[1] = -1.0 * p / 2.0;
  494.    cubic[2] = -1.0 * r;
  495.    cubic[3] = r * p / 2.0 - q * q / 8.0;
  496.    k = solve_cubic(&cubic[0], &roots[0]);
  497.    z = roots[0];
  498.    if (k == 3)
  499.       z = max(max(roots[0], roots[1]), roots[2]);
  500.    else
  501.       z = roots[0];
  502.    d1 = 2.0 * z - p;
  503.    if (d1 < 0.0)
  504.       return 0;
  505.    else if (d1 == 0.0) {
  506.       d2 = z * z - r;
  507.       if (d2 < 0.0) return 0;
  508.       d2 = sqrt(d2);
  509.       }
  510.    else {
  511.       d1 = sqrt(d1);
  512.       d2 = 0.5 * q / d1;
  513.       }
  514.    i = 0;
  515.    cubic[0] = 1.0;
  516.    cubic[1] = d1;
  517.    cubic[2] = z - d2;
  518.    j = solve_quadratic(&cubic[0], &roots[0]);
  519.    if (j==1)
  520.       results[i++] = roots[0] - x[1] / 4.0;
  521.    else if (j == 2) {
  522.       results[i++] = roots[0] - x[1] / 4.0;
  523.       results[i++] = roots[1] - x[1] / 4.0;
  524.       }
  525.    cubic[1] = -d1;
  526.    cubic[2] = z + d2;
  527.    j = solve_quadratic(&cubic[0], &roots[0]);
  528.    if (j == 0)
  529.       return i;
  530.    else if (j==1)
  531.       results[i++] = roots[0] - x[1] / 4.0;
  532.    else {
  533.       results[i++] = roots[0] - x[1] / 4.0;
  534.       results[i++] = roots[1] - x[1] / 4.0;
  535.       }
  536.    return i;
  537. }
  538.  
  539.  
  540. #ifdef TEST
  541.  
  542. #ifndef __TURBOC__
  543. int _FAR_ _cdecl
  544. dbl_sort_func(const void _FAR_ *a, const void _FAR_ *b)
  545. #else
  546. int
  547. dbl_sort_func(a, b)
  548.    void *a, *b;
  549. #endif
  550. {
  551.    return *((DBL *)a) - *((DBL *)b);
  552. }
  553.  
  554. /*
  555.    Solve the nth order equation:
  556.       x[0] x^n + x[1] x^(n-1) + ... + x[n-1] x + x[n] = 0
  557.    The value of this function is the number of real roots. The roots
  558.    themselves are returned in roots[0-(n-1)]. The roots will be in sorted
  559.    order when they are returned.
  560. */
  561. int
  562. polysolve(order, Coeffs, roots)
  563.    int order;
  564.    DBL *Coeffs;
  565.    DBL *roots;
  566. {
  567.    DBL val, dx;
  568.    DBL *inflections, *derivative, root;
  569.    register int i, j;
  570.    int ipoints, rootcount;
  571.    if (order == 2) {
  572.       rootcount = solve_quadratic(Coeffs, roots);  
  573.       if (rootcount == 2 && roots[0] > roots[1]) {
  574.      root = roots[0];
  575.      roots[0] = roots[1];
  576.      roots[1] = root;
  577.      }
  578.       }
  579.    else if (order == 3) {
  580.       rootcount = solve_cubic(Coeffs, roots);
  581. #ifndef __TURBOC__
  582.       qsort((void _FAR_ *)roots, rootcount, sizeof(DBL), dbl_sort_func);
  583. #else
  584.       qsort((void *)roots, rootcount, sizeof(DBL), dbl_sort_func);
  585. #endif
  586.       }
  587.    else if (order == 4) {
  588.       rootcount = solve_quartic(Coeffs, roots);
  589.       qsort((void *)roots, (size_t)rootcount, sizeof(DBL), dbl_sort_func);
  590.       }
  591.    else {
  592.       inflections = (DBL *)malloc((order+1) * sizeof(DBL));
  593.       if (!inflections) {
  594.      printf("Failed to allocate memory for inflections in polysolve\n");
  595.      exit(1);
  596.      }
  597.       derivative = (DBL *)malloc(order * sizeof(DBL));
  598.       if (!derivative) {
  599.      printf("Failed to allocate memory for derivative in polysolve\n");
  600.      exit(1);
  601.      }
  602.       dydx(order, Coeffs, derivative);
  603.       /* show_poly("Derivs  : ", order-1, derivative); */
  604.       ipoints = polysolve(order-1, derivative, &inflections[1]);
  605.       /* show_points("Inflect : ", ipoints, &inflections[1]); */
  606.       /* Strip out redundant inflection points */
  607.       for (i=1;i<ipoints;)
  608.      if (inflections[i] == inflections[i+1]) {
  609.         for (j=i;j<ipoints;j++)
  610.            inflections[j] = inflections[j+1];
  611.         ipoints--;
  612.         }
  613.      else
  614.         i++;
  615.       /* show_points("Stripped: ", ipoints, &inflections[1]); */
  616.       /* Add negative and positive infinity to the list */
  617.       inflections[0] = MINUS_INFINITY;
  618.       inflections[ipoints+1] = POSITIVE_INFINITY;
  619.       /* Use bounded Newton-Raphson to solve for roots between points of
  620.          inflection. (When their values differ in sign.) */
  621.       rootcount = 0;
  622.       for (i=0;i<=ipoints;i++)
  623.      if (rtsafe(polyeval, order, Coeffs, inflections[i],
  624.             inflections[i+1], EPSILON, &root))
  625.         roots[rootcount++] = root;
  626.       /* Free up the temporary memory */
  627.       free(derivative);
  628.       free(inflections);
  629.       }
  630.    return rootcount;
  631. }
  632. #endif
  633.